#include <ros/ros.h>
#include <autodrive_msgs/NavPacket.h>
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <arpa/inet.h>

#define MAXLINE 40
#define UDPPORT 10001
#define SERVERIP "127.0.0.1"

struct SendData {
  uint32_t msg_id;
  uint32_t msg_index;
  uint32_t len;
  uint32_t timestamp;
  int32_t longitude;
  int32_t latitude;
  uint16_t heading;
  int16_t pitch;
  int16_t row;
  uint16_t altitude;
  int16_t velocity;
  uint16_t team_id;
  uint32_t crc;
};

class UdpSend
{
  private:
    ros::NodeHandle nh;
    ros::Subscriber subNav;

    int confd;
    int sockfd;
    unsigned int addr_length;

    struct sockaddr_in addr;
    socklen_t addr_len;
    char sendline[MAXLINE];

    SendData* send_data;
    uint32_t m_index = 0;

  public:
    UdpSend(/* args */);
    ~UdpSend();

    void NavCallBack(const autodrive_msgs::NavPacket::Ptr &Navmsg);

    void sendData();
};

uint32_t getCRC(char* buf, uint16_t len);
